#include "arm.hpp"
#include "log.h"
#include "tv_interface.hpp"

#define LOGTAG "arm"

//queue motion

arm::arm(Stepper &stepper1,Stepper &stepper2):stepperX(stepper1), stepperY(stepper2)
{
    this->mode = ARM_MODE_IDLE;
    this->status = ARM_IDLE_STATUS;

}

void arm::run(void)
{
    switch(this->mode)
    {
        case ARM_MODE_IDLE:
        {
            
        }break;
        case ARM_MODE_RESET:
        {
            this->runReset();
        }break;
        case ARM_MODE_GOAIM:
        {
            this->runGoAim();
        }break;
        case ARM_MODE_USERHOOK:
        {
            this->runUserHook();
        }break;
        default:
        {
            //Do default
            this->mode = ARM_MODE_IDLE;
        }break;
    }
}

void arm::runReset(void)
{
    switch (this->status)
    {
        case ARM_RESET_READY:
        {
            this->stepperX.setSpeed(this->resetConfig.speedX);
            this->stepperY.setSpeed(this->resetConfig.speedY);
            if (this->stepperX.limitsw1.getState())
            {
                /* 偏移然后重新复位一次 */
                this->stepperX.limitStepDisable();
                this->stepperX.moveStep(this->resetConfig.againResetOffsetStepX); 
                this->status = ARM_RESET_WAIT;
            }
            if (this->stepperY.limitsw1.getState())
            {
                this->stepperY.limitStepDisable();
                this->stepperY.moveStep(this->resetConfig.againResetOffsetStepY); 
                this->status = ARM_RESET_WAIT;
            }
            if (this->status == ARM_RESET_WAIT) return;
            this->stepperX.reset(this->resetConfig.dirX);
            this->stepperY.reset(this->resetConfig.dirY);
            this->status = ARM_RESET_DOING;
            LOG_INFO(LOGTAG, "ARM Reset Start...\n");
        }
        break;
        case ARM_RESET_WAIT:
        {
            if (this->stepperX.getMoveState() == STEPPER_DRIVER_OK
             && this->stepperY.getMoveState() == STEPPER_DRIVER_OK)
            {
                LOG_INFO(LOGTAG, "ARM Reset again\n");
                this->status = ARM_RESET_READY;
            }
        }break;
        case ARM_RESET_DOING:
        {
            if (this->stepperX.limitsw1.getState() && this->stepperY.limitsw1.getState())
            {
                if (this->resetConfig.curentResetCnt > 0)
                {
                    this->resetConfig.curentResetCnt--;
                    this->status = ARM_RESET_READY;
                    return;
                }
                LOG_INFO(LOGTAG, "ARM Reset Cplt\n");
                this->status = ARM_RESET_CPLT;
            }
            if (this->stepperX.getMoveState() == 0 && this->stepperY.getMoveState() == 0)
            {
                this->stepperX.reset(this->resetConfig.dirX);
                this->stepperY.reset(this->resetConfig.dirY);
            }
        }
        break;
        case ARM_RESET_CPLT:
        {
            this->mode = ARM_MODE_IDLE;
        }
        break;
        default:
        {
            // error
            LOG_ERR(LOGTAG, "Error Status(%d) in arm reset mode\n", this->status);
        }
        break;
    }
}

void arm::runGoAim(void)
{
    switch (this->status)
    {
        case ARM_GOAIM_READY:
        {
            this->stepperX.setSpeed(this->goaimConfig.speedX);
            this->stepperY.setSpeed(this->goaimConfig.speedY);
            this->stepperX.moveToStep(this->goaimConfig.stepX);
            this->stepperY.moveToStep(this->goaimConfig.stepY);
            this->status = ARM_GOAIM_DOING;
            LOG_INFO(LOGTAG, "Go Aim Start...\n");
        }
        break;
        case ARM_GOAIM_DOING:
        {
            if (this->stepperX.getStep() == this->goaimConfig.stepX
             && this->stepperY.getStep() == this->goaimConfig.stepY)
            {
                this->status = ARM_GOAIM_CPLT;
                LOG_INFO(LOGTAG, "Go Aim Cplt\n");
            }
        }
        break;
        case ARM_GOAIM_CPLT:
        {
            this->mode = ARM_MODE_IDLE;
            if (goaimConfig.endHook)
            {
                goaimConfig.endHook();
                goaimConfig.endHook = NULL;
            }
        }
        break;
        default:
        {
            // error
            LOG_ERR(LOGTAG, "Error Status(%d) in arm GoAim mode\n", this->status);
        }
        break;
    }
}

void arm::runUserHook(void)
{
    if (this->status >= ARM_HOOK_READY && this->status <= ARM_HOOK_CPLT)
    {
        if (userHookConfig.doingHook) userHookConfig.doingHook(this->status);
        else stopUserHook();
    }
    else
    {
        // error
        LOG_ERR(LOGTAG, "Error Status(%d) in UserHook mode\n", this->status);
        stopUserHook();
    }
}


void arm::configResetSpeed(float speedx, float speedy)
{
    if (speedx == 0 || speedy == 0)
    {
        // Log here
        LOG_WARN(LOGTAG, "speed can't be zero\n");
        return;
    }
    if (speedx > 0)
    {
        this->resetConfig.dirX = 1;
        this->resetConfig.speedX = speedx;
    }
    else
    {
        this->resetConfig.dirX = -1;
        this->resetConfig.speedX = -speedx;
    }
    if (speedy > 0)
    {
        this->resetConfig.dirY = 1;
        this->resetConfig.speedY = speedy;
    }
    else
    {
        this->resetConfig.dirY = -1;
        this->resetConfig.speedY = -speedy;
    }
}
void arm::configResetOffset(int offsetx, int offsety)
{
    this->resetConfig.againResetOffsetStepX = offsetx;
    this->resetConfig.againResetOffsetStepY = offsety;
}
void arm::configResetOffset(float offsetAnglex, float offsetAngley)
{
    this->resetConfig.againResetOffsetStepX = this->stepperX.angleToStep(offsetAnglex);
    this->resetConfig.againResetOffsetStepY = this->stepperY.angleToStep(offsetAngley);
}
void arm::startReset(void)
{
    if (this->mode != ARM_MODE_IDLE)
    {
        // Log here
        LOG_WARN(LOGTAG, "Reset faild, arm current is busy (Mode:%d,Status:%d)\n",this->mode,this->status);
        return;
    }
    this->resetConfig.curentResetCnt = this->resetConfig.resetCnt;
    this->mode = ARM_MODE_RESET;
    this->status = ARM_RESET_READY;
}
void arm::stopReset(void)
{
    if (this->mode != ARM_MODE_RESET)
    {
        // Log here
        LOG_WARN(LOGTAG, "arm is not Reset Mode (Mode:%d,Status:%d)\n",this->mode,this->status);
        return;
    }
    this->stepperX.stop();
    this->stepperY.stop();
    this->mode = ARM_MODE_IDLE;
}

void arm::configResetCnt(uint8_t resetCount)
{
    this->resetConfig.resetCnt = resetCount;
}

void arm::configGoAimSpeed(float speedx, float speedy)
{
    this->goaimConfig.speedX = speedx;
    this->goaimConfig.speedY = speedy;
    this->stepperX.setSpeed(this->goaimConfig.speedX);
    this->stepperY.setSpeed(this->goaimConfig.speedY);
}
void arm::configGoAimXY(long aimx, long aimy)
{
    this->goaimConfig.stepX = aimx;
    this->goaimConfig.stepY = aimy;
}
void arm::startGoAim(long aimx, long aimy)
{
    if (this->mode != ARM_MODE_IDLE)
    {
        // Log here
        LOG_WARN(LOGTAG, "GoAim faild, arm current is busy (Mode:%d,Status:%d)\n",this->mode,this->status);
    }
    this->goaimConfig.stepX = aimx;
    this->goaimConfig.stepY = aimy;
    this->mode = ARM_MODE_GOAIM;
    this->status = ARM_GOAIM_READY;
}
void arm::startGoAim(void)
{
    if (this->mode != ARM_MODE_IDLE)
    {
        // Log here
        LOG_WARN(LOGTAG, "GoAim faild, arm current is busy (Mode:%d,Status:%d)\n",this->mode,this->status);
    }
    this->mode = ARM_MODE_GOAIM;
    this->status = ARM_GOAIM_READY;
}
void arm::stopGoAim(void)
{
    if (this->mode != ARM_MODE_GOAIM)
    {
        // Log here
        LOG_WARN(LOGTAG, "arm is not GoAim Mode (Mode:%d,Status:%d)\n",this->mode,this->status);
        return;
    }
    this->stepperX.stop();
    this->stepperY.stop();
    this->mode = ARM_MODE_IDLE;
}


void arm::startUserHook(void)
{
    if (this->mode != ARM_MODE_IDLE)
    {
        LOG_WARN(LOGTAG, "UserHook faild, arm current is busy (Mode:%d,Status:%d)\n",this->mode,this->status);
    }
    this->mode = ARM_MODE_USERHOOK;
    this->status = ARM_HOOK_READY;
	if (userHookConfig.startHook) userHookConfig.startHook();
}
void arm::stopUserHook(void)
{
    if (userHookConfig.stopHook) userHookConfig.stopHook();
    this->stepperX.stop();
    this->stepperY.stop();
    this->mode = ARM_MODE_IDLE;
}
void arm::stop(void)
{
    switch (this->mode)
    {
    case ARM_MODE_RESET:
        this->stopReset();
        break;
    case ARM_MODE_GOAIM:
        this->stopGoAim();
        break;
    case ARM_MODE_USERHOOK:
        this->stopUserHook();
        break;
    default:
        break;
    }
}

Line_polyfit2_t pf2_y = {-0.07898, -0.4286, -1.504, 20.48, 512.9 , -1.471e+04 + 300};
Line_polyfit2_t pf2_x = {-1.475, 0.02891, -0.01047, 531.4, 2.526, 5616};
armPoint_t arm::coodinate2step(double x, double y)
{
    armPoint_t p = {0, 0};
    p.x = polyeval2(x, y, &pf2_x);
    p.y = polyeval2(x, y, &pf2_y);
    return p;
}
armPoint_t arm::cam2step(double x, double y)
{
    armPoint_t p = {0, 0};
    p.x = x / 4.00 + 1;
    p.y = y / 4.00 - 4;
    // Queue_Printf(&htx1, "camAxis2step(%.1f,%.1f)to(%.2f,%.2f)\r\n", x, y, p.x, p.y);
    return coodinate2step(p.x, p.y);
}

void arm::trackObject(trackResult_t *result)
{
    if (result == NULL) return;
    armPoint_t p = cam2step(result->sx + (result->w>>1), result->sy + (result->h>>1));
    startGoAim(p.x, p.y);
}
/*
void arm::trackObject(trackResultCollecter &objs)
{
    int index = -1;
    objs.keepResult(objs.trackConfig);
    if (objs.cnt == 0)
        return;
    index = objs.findMaxCountObj();
    if (index < 0)
        return;
    trackResult_t *result = objs.readResult(index);
    armPoint_t p = cam2step(result->sx + (result->w>>1), result->sy + (result->h>>1));
    TVAPI.pauseVideo();
    startGoAim(p.x, p.y);
}
*/
void arm::startGoAimCoodinate(double x, double y)
{
    armPoint_t p = coodinate2step(x, y);
    startGoAim(p.x, p.y);
}
